#include "SDM.h"
#include "Polygon.h"
#include "AABB.h"
#include <math.h>

#define POS_INF	1e10

SDM::SDM(void)
{
}


SDM::~SDM(void)
{
}

BruteForceSDM::BruteForceSDM( void )
{

}

BruteForceSDM::BruteForceSDM( Polygon* p )
{
	poly = p;
}

BruteForceSDM::~BruteForceSDM( void )
{

}

Value BruteForceSDM::getEvaluation( Vec2d p, bool compute_normal )
{
	double dist;
	Vec2d closestPt, normal;

	poly->getShortestDistanceFromPointToPoly(p, &dist, &closestPt, & normal);

	Value v;
	v.p = p;
	v.d = dist;
	v.n = normal;

	return v;
}

GridSDM::GridSDM( void )
{

}

GridSDM::GridSDM( Polygon* poly, double cellSize )
{
	this->cellSize = cellSize;
	this->invCellSize = 1.0 / cellSize;
	bfsdm = new BruteForceSDM(poly);

	bbox = new AABB();

	for (int i=0; i<poly->vertCount; i++)
	{
		Vert v = poly->vertList[i];

		bbox->Expand(Vec2d(v.x, v.y));
	}

	Vec2d extents = bbox->p1 - bbox->p0;

	w = (int)ceil(extents[0] * invCellSize) + 3;
	h = (int)ceil(extents[1] * invCellSize) + 3;	

	grid = (double*)malloc(w * h * sizeof(double));
	gridInitialized = (bool*)malloc(w * h * sizeof(bool));

	for (int i=0; i<w*h; i++)
	{
		gridInitialized[i] = false;
	}
}

GridSDM::~GridSDM( void )
{

}

double GridSDM::getGridData( int x, int y )
{
	return grid[x * h + y];
}

void GridSDM::setGridData( int x, int y, double v )
{
	grid[x * h + y] = v;
}

bool GridSDM::getGridInitData( int x, int y )
{
	return gridInitialized[x * h + y];
}

void GridSDM::setGridInitData( int x, int y, bool v )
{
	gridInitialized[x * h + y] = v;
}

double GridSDM::getGridValue( int x, int y )
{
	x++; y++;

	if (!getGridInitData(x, y))
	{
		setGridData(x, y, bfsdm->getEvaluation(Vec2d(x-1, y-1) * cellSize + bbox->p0, true).d);
		setGridInitData(x, y, true);		
	}

	return getGridData(x, y);
}

Value GridSDM::getEvaluation( Vec2d p, bool compute_normal )
{
	Value v;	
	v.d = POS_INF;

	// check against bbox
	if (!bbox->Contains(p)) return v;

	Vec2d ptRel = (p - bbox->p0) * invCellSize;
	int x = (int)floor(ptRel[0]);
	int y = (int)floor(ptRel[1]);
	
	Vec2d ptOff = ptRel - Vec2d(x, y);

	// trilerp to get the value
	v.d = 
		(getGridValue(x+0,y+0) * (1-ptOff[0]) + getGridValue(x+1,y+0) * (0+ptOff[0])) * (1-ptOff[1]) + 
		(getGridValue(x+0,y+1) * (1-ptOff[0]) + getGridValue(x+1,y+1) * (0+ptOff[0])) * (0+ptOff[1]);

	if (compute_normal)
	{
		// bilerp to get the gradient in each dimension
		double dfdx = 0;
		double debug;
		
		dfdx += (getGridValue(x+1,y+0) - getGridValue(x+0,y+0)) * (1-ptOff[1]);
		dfdx += (getGridValue(x+1,y+1) - getGridValue(x+0,y+1)) * (0+ptOff[1]);		

		double dfdy = 0;
		dfdy += (getGridValue(x+0,y+1) - getGridValue(x+0,y+0)) * (1-ptOff[0]);		
		dfdy += (getGridValue(x+1,y+1) - getGridValue(x+1,y+0)) * (0+ptOff[0]);		

		v.n = Vec2d(1,0) * dfdx + Vec2d(0,1) * dfdy;

		v.n = normalized(v.n);
	}

	return v;
}
